COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00005 00003	⊃ PROCEDURE FOR SAVING A INTEGER NUMBER IN THE DATA FILE
C00007 00004	PROCEDURE MOVE(INTEGER BITS REAL ARRAY B REAL TT(2.0) INTEGER P(0))
C00010 00005	PROCEDURE DRIVE(STRING COLOR INTEGER J REAL OLDJ,NEWJ,TT(2.0))
C00013 00006	PROCEDURE OPEN_A(STRING COLOR REAL NEWH,TT(2.0))
C00014 00007	PROCEDURE CENTER(INTEGER BITS)
C00015 ENDMK
C⊗;
ENTRY;

BEGIN "MOVE  - GENERATES SIMPLE TRAJECTORY AND SENDS DATA TO BLUE ARM"
COMMENT ALL [PNT,HE] FILES COPIED OVER FROM [PNT,MSM] DEC 30,1977 ;
DEFINE ⊃="COMMENT",CR="'15",LF="'12",CRLF="('15&'12)",FF="'14";

INTEGER SEGS,TNUM,TRANS,JTS,I,J,K;
PRELOAD_WITH 1,2,3,4,5,6,7;
INTEGER ARRAY JT[1:7],DATA[1:1000];
INTEGER DUM, PTR;
REAL ARRAY DD[1:20],OLD[1:7],NEW[1:7];
INTERNAL REAL BHAND;

EXTERNAL PROCEDURE DTERMS(REAL ARRAY DD;REFERENCE REAL TH;INTEGER ARM);
EXTERNAL PROCEDURE TLKEF3(INTEGER MASTER;INTEGER ARRAY DATA);
EXTERNAL INTEGER PROCEDURE TLKEF5(REAL ARRAY A,B);
EXTERNAL PROCEDURE ARMSOL(REAL ARRAY A,B; INTEGER P);

REQUIRE "BEJCZY[PNT,HE]" LOAD_MODULE; COMMENT ACTUALLY FROM BEJCZY[11,BES];
REQUIRE "FAITRG.FAI[PNT,HE]" LOAD_MODULE; COMMENT FROM 1,BES;
REQUIRE "TLKF3a.FAI[PNT,HE]" LOAD_MODULE; COMMENT FROM 11,BES;
REQUIRE "TLKF5a.FAI[PNT,HE]" LOAD_MODULE; COMMENT FROM 11,BES;
REQUIRE "ARMSOL.SAI[PNT,HE]" LOAD_MODULE;

DEFINE	MASTER="'54321";
DEFINE  MOVE_CODE= "'76";
DEFINE  CENTER_CODE= "'67";

PROCEDURE GET_JOINTS(STRING COLOR; REAL ARRAY A);
	BEGIN
	OWN REAL ARRAY BESTNS[1:4,1:3], BESANGLES[1:7];
	IF EQU(COLOR,"BLUE")
	THEN BEGIN
		INTEGER I;
		IF TLKEF5(BESTNS,BESANGLES) THEN
			OUTSTR("ERROR IN READING ARM");
		FOR I←1 STEP 1 UNTIL 7 DO A[I]←BESANGLES[I];
		BHAND←BESANGLES[7];
	     END
	ELSE OUTSTR("CANT READ YELLOW ARM YET");
	END;

⊃ PROCEDURE FOR SAVING A INTEGER NUMBER IN THE DATA FILE;

SIMPLE PROCEDURE INTOUT(INTEGER NUM);
	BEGIN
	DATA[PTR]←NUM;
	PTR←PTR+1;
	END;




⊃ PROCEDURE FOR SAVING A FLOATING POINT NUMBER IN 11 FORMAT IN THE DATA ARRAY;

SIMPLE PROCEDURE FLTOUT(REAL FNUM);
	BEGIN
	LABEL ST1,ST2,OVER,FLTEND;
	INTEGER BYTE,NUM1,NUM2;
	BYTE←'013200000002;
		START_CODE
		   	MOVE   2,FNUM;
			JUMPGE 2,ST1;
			MOVN   2,2;
 			TLO    2,'400000;
		ST1:	JFCL   2,ST2;
		ST2:	ADDI   2,4;
			JFCL   2,OVER;
     		    	DPB    2,BYTE;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM1;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM2;
		END;
	DATA[PTR]←NUM1;
	PTR←PTR+1;
	DATA[PTR]←NUM2;
	PTR←PTR+1;
	GOTO FLTEND;
OVER:	OUTSTR("ERROR-ROUNDING OVERFLOW"&CRLF);
FLTEND:	END; 
PROCEDURE MOVE(INTEGER BITS; REAL ARRAY B; REAL TT(2.0); INTEGER P(0));
BEGIN "MOVE"
	INTEGER JTS,TIME,SEGPTR,JOINT;
	REAL DIF;
	IF BITS='770
		THEN BEGIN DUM←0; GET_JOINTS("BLUE", OLD); END
		ELSE BEGIN DUM←1; GET_JOINTS("YELLOW",OLD); END;
⊃ DUM SHOULD BE 0 FOR BLUE ARM;
⊃ SET UP THE COEFFICIENT LIST HEADER;
	SETFORMAT(10,3);
	JTS←6;
	SEGPTR←8+JTS*32;
	PTR←1;
⊃ ***** ; INTOUT(MOVE_CODE);
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0); ⊃ WOBBLE;

	ARMSOL(NEW,B,P); ⊃ GIVES THE ANGLES OF THE NEW TRANSFORM;

			TIME←TT*1000;  ⊃ SEGMENT TIME IN MSEC;
			INTOUT(SEGPTR);
			INTOUT(TIME);
			INTOUT(0); INTOUT(0);

⊃ WRITE OUT THE POLYNOMIAL AND DYNAMIC COEFFICIENTS;

			FOR J←1 STEP 1 UNTIL JTS DO BEGIN
				JOINT←JT[J];
				DIF←NEW[JOINT]-OLD[JOINT];
				FLTOUT(OLD[JOINT]);
				FLTOUT(0.0);  FLTOUT(0.0);
				FLTOUT(10.0*DIF);
				FLTOUT(-15.0*DIF);
				FLTOUT(6.0*DIF);
			END;
			DTERMS(DD,NEW[1],DUM);
			FOR J←1 STEP 1 UNTIL JTS DO BEGIN
				K←(JT[J]-1)*2+1;
				FLTOUT(DD[K]);
				FLTOUT(DD[K+1]);
			END;
			INTOUT(0);

		TLKEF3(MASTER,DATA); ⊃ tell to move;

END "MOVE";

INTERNAL PROCEDURE MOVE_B(REAL ARRAY B; integer p(0);REAL TT(2.0));
	MOVE('770,B,TT,P);

INTERNAL PROCEDURE MOVE_Y(REAL ARRAY B; integer p(0) ;real tt(2.0));
	MOVE('176000,B,TT,P);

	PRELOAD_WITH '400,'200,'100,'40,'20,'10,'4;
	INTEGER ARRAY BJT_CODE[1:7];
	PRELOAD_WITH '100000,'40000,'20000,'10000,'4000,'2000,'1000;
	INTEGER ARRAY YJT_CODE[1:7];
PROCEDURE DRIVE(STRING COLOR; INTEGER J; REAL OLDJ,NEWJ,TT(2.0));
BEGIN "DRIVE"
	⊃ DRIVES ONE JOINT;
	INTEGER JTS,SEGPTR,TIME,BITS,K1;
	REAL DIF;
	IF EQU(COLOR,"BLUE") 
		THEN BEGIN IF J=7 THEN DUM←8 ELSE DUM←4;
			   BITS←BJT_CODE[J]; END
		ELSE BEGIN IF J=7 THEN DUM←2 ELSE DUM←1;
			   BITS←YJT_CODE[J]; END;
	JTS←1; PTR←1; SEGPTR←8+JTS*32;
	SETFORMAT(10,3);
	INTOUT(MOVE_CODE);
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0);
	TIME←TT*1000;
	INTOUT(SEGPTR);
	INTOUT(TIME);
	INTOUT(0);
	INTOUT(0);
	FLTOUT(OLDJ);
	FLTOUT(0.0);FLTOUT(0.0);
	DIF←NEWJ-OLDJ;
	FLTOUT(10.0*DIF);
	FLTOUT(-15.0*DIF);
	FLTOUT(6.0*DIF);
	FOR K1←1 STEP 1 UNTIL 7 DO NEW[K1]←OLD[K1];
	NEW[J]←NEWJ;
	DTERMS(DD,NEW[1],DUM);
	IF J=7 THEN K←1 ELSE K←(JT[J] -1)*2 +1;
	FLTOUT(DD[K]);
	FLTOUT(DD[K+1]);
	INTOUT(0);
	TLKEF3(MASTER,DATA); ⊃ TELL TO MOVE;
END "DRIVE";

PROCEDURE DRIVE_DEL(STRING COLOR; INTEGER J; REAL DIF,TT(2.0));
BEGIN "DRIVE_DEL"
	REAL ARRAY OLD[1:7];
	GET_JOINTS(COLOR,OLD);
	DRIVE(COLOR,J,OLD[J],OLD[J]+DIF,TT);
END "DRIVE_DEL";

PROCEDURE DRIVE_ABS(STRING COLOR; INTEGER J; REAL NEWJ,TT(2.0));
BEGIN "DRIVE_ABS"
	REAL ARRAY OLD[1:7];
	GET_JOINTS(COLOR,OLD);
	DRIVE(COLOR,J,OLD[J],NEWJ,TT);
END "DRIVE_ABS";

INTERNAL PROCEDURE DR_B_D(INTEGER J; REAL DIF,TT(2.0));
	DRIVE_DEL("BLUE",J,DIF,TT);

INTERNAL PROCEDURE DR_Y_D(INTEGER J; REAL DIF,TT(2.0));
	DRIVE_DEL("YELLOW",J,DIF,TT);

INTERNAL PROCEDURE DR_B_A(INTEGER J; REAL NEWJ,TT(2.0));
	DRIVE_ABS("BLUE",J,NEWJ,TT);

INTERNAL PROCEDURE DR_Y_A(INTEGER J; REAL NEWJ,TT(2.0));
	DRIVE_ABS("YELLOW",J,NEWJ,TT);

PROCEDURE OPEN_A(STRING COLOR; REAL NEWH,TT(2.0));
BEGIN "OPEN_ABS"
	REAL ARRAY OLD[1:7];
	GET_JOINTS(COLOR,OLD);
	DRIVE(COLOR,7,OLD[7],NEWH,TT);
END "OPEN_ABS";

PROCEDURE OPEN_D(STRING COLOR; REAL DIF,TT(2.0));
BEGIN "OPEN_DEL"
	REAL ARRAY OLD[1:7];
	GET_JOINTS(COLOR,OLD);
	DRIVE(COLOR,7,OLD[7],OLD[7]+DIF,TT);
END "OPEN_DEL";

INTERNAL PROCEDURE OPNB_D(REAL DIF,TT(2.0));
	OPEN_D("BLUE",DIF,TT);

INTERNAL PROCEDURE OPNB_A(REAL NEWH,TT(2.0));
	OPEN_A("BLUE",NEWH,TT);

PROCEDURE CENTER(INTEGER BITS);
BEGIN "CENTER"
	PTR←1;
	INTOUT(CENTER_CODE);
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0);	⊃ NO WOBBLE;
	INTOUT(0);	⊃ NO NEXT SEGMENT;
	INTOUT(0);	⊃ NO FUNCTION TIME;
	INTOUT(0);	⊃ NO TRANSFORM;
	INTOUT(0);
	INTOUT(0);
	TLKEF3(MASTER,DATA);
END "CENTER";

INTERNAL PROCEDURE CENT_B;
	CENTER('774);
INTERNAL PROCEDURE CENT_Y;
	CENTER('177000);


END;